///////////////////////////////////////////////////////////
// Project 2 //
// Andrew Hains //
// Joseph Blackwell //
// Robert Smyly //
//////////////////////////
// Roomba Controller //
// MAIN //
// Allows a roomba to be controlled via bluetooth //
// Using keyboard commands and sensor input //
///////////////////////////////////////////////////////////

#include <stdio.h>
#include <stdlib.h>
#include <time.h>
#include <pthread.h>
#include <unistd.h>
#include "robot.h"

#define DEFAULT_DEVICE "/dev/rfcomm0"
#define BUMPER_CHECK_RATE 20 //Hz (must be > 1)

time_t start_time; //the time that the program started

//checks the bumper sensors and prints the results with a timestamp (number of seconds since the program started
void check_bumpers() {
	int rate;
	char bumper;
	time_t stamp;
	
	rate = 1000000/BUMPER_CHECK_RATE;
	while(1) {
		robot_bumpers(&bumper, &stamp);
		printf("bumpers: ");
		if(bumper < 0 || bumper > 3) {
			printf("not valid data\n");
		}
		printf("left=%-3s   ", ((bumper & 2) == 2) ? "yes" : "no");
		printf("right=%-3s   ", ((bumper & 1) == 1) ? "yes" : "no");
		printf("time=%d\n", (int) difftime(stamp, start_time));
		usleep(rate);
	}
}

//runs user interface
void *interface_function(void *param) {
	char c;

	while(1) {
		scanf("%c", &c);
		if(c == 'q') {
			robot_close();
			exit(0);
		}
		else if(c == 'f') {
			robot_forward();
		}
		else if(c == 'b') {
			robot_backward();
		}
		else if(c == 'l') {
			robot_left();
		}
		else if(c == 'r') {
			robot_right();
		}
		else if(c == 's') {
			robot_stop();
		}
	}
}

//main function
int main(int argc, char **argv) {
	char *device = DEFAULT_DEVICE;
	pthread_t interface_thread; //thread that the user interface runs in

	start_time = time(NULL); //records the time that the program started

	if(argc > 1) { //first command line argument is the serial device used. if not provided, defaults to DEFAULT_DEVICE
		device = argv[1];
	}

	printf("attempting to connect to %s\n", device);

	if(robot_init(device) < 0) { //failed to initialize robot, or one of the structures needed to operate it
		fprintf(stderr, "could not connect\n");
		exit(1);
	}
	printf("connected\n");

	pthread_create(&interface_thread, NULL, interface_function, NULL); //start the interface in a new thread
	check_bumpers();
}
